#include "gray.h"


/*------------------------ 通过灰度判断位置偏差 --------------------*/

/* 前位置偏差，曾经用1和-1，因为底盘不对称，所以左右控制量不同 */
signed char gray_front_error(void)
{
	u8 temp = (GRAY_F_L << 2) | (GRAY_F_M << 1) | GRAY_F_R;   //灰度拼接起来
	signed char err;		/* 位置偏差 */
	
	switch (temp)
	{
		case 0x03:          // 011 白线，左大
			err = -27;
			break;
		case 0x06:          // 110 白线，右大
			err = 23;
			break;
		case 0x00:          // 000 无白线
			err = 0;
			break;
		default:						// 101 白线在中间，111 跑出去了，有两个同时亮。
			err = 0;
			break;
	}
	return err;
}

/* 后位置偏差 */
signed char gray_back_error(void)
{
	signed char err;
  u8 temp = (GRAY_B_L << 2) | (GRAY_B_M << 1) | GRAY_B_R;
  
   switch (temp)
  {
    case 0x03:          //011偏右+
      err = -23;
      break;
    case 0x06:          //110偏左+
      err = 27;
      break;
    default: 
      err = 0;
      break;
  } 
	return err;
}

/* 判断是否是十字路口或十字标线，左右都为0 */
bool gray_is_cross(void)
{
  return
      ((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
       (GRAY_R_F && GRAY_R_M && GRAY_R_B)) == 0;
}

/* 判断是否为T字路口，前全1，左右有0 */
bool gray_is_t(void)
{
  return (!(GRAY_F_L && GRAY_F_M && GRAY_F_R) ||
          ((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
           (GRAY_R_F && GRAY_R_M && GRAY_R_B))) == 0;
}

/* 左全1，右有0 */
bool gray_is_right_t(void)
{
  return ( 
         (!(GRAY_L_F && GRAY_L_M && GRAY_L_B))	||
           (GRAY_R_F && GRAY_R_M && GRAY_R_B)) == 0;
}

/* 左有0 */
bool gray_is_left_corner(void)
{
  return (GRAY_L_M) == 0;
}

bool gray_is_right_corner(void)
{
  return (GRAY_R_M) == 0;
}

/* 前停止线，全是0 */
bool gray_front_stop_line(void)
{
  return (((GRAY_F_L&&GRAY_F_R) == 0)&&((GRAY_R_F||GRAY_R_M)==0));
}

/* 判断转弯是否到位，可以直接用十字路口逻辑，这个条件更宽松 */
bool gray_turn_not_ok0(void)
{
  /* 车头对正白线，转弯就可以停止 */  
  return (GRAY_F_M);  
}


/* 编译选项，去掉编码格式带来的警告 */
#pragma  diag_suppress 870

void gray_single_test(void)
{
  while(1)
  {
    printf("前左：%d 前中：%d 前右：%d \n", GRAY_F_L,GRAY_F_M,GRAY_F_R);
    printf("后左：%d 后中：%d 后右：%d \n", GRAY_B_L,GRAY_B_M,GRAY_B_R);
    printf("左前：%d 左中：%d 左后：%d \n", GRAY_L_F,GRAY_L_M,GRAY_L_B);
    printf("右前：%d 右中：%d 右后：%d \n", GRAY_R_F,GRAY_R_M,GRAY_R_B);
    
    printf("=========================================== \n");
    
    delay_ms(500);
  }
}

void gray_test(void)
{
	while(1)
	{
		if (gray_is_cross()==1)
			printf("cross\n");
		if (gray_is_t()==1)
			printf("T\n");
		if (gray_turn_not_ok0() == 0 )
			printf("ok0\n");
		if (gray_is_right_t() == 1)
			printf("right_t\n");
		if (gray_is_right_corner() == 1)
			printf("right corner\n"); 
		if (gray_is_left_corner() == 1)
			printf("left corner\n"); 

		delay_ms(500);
		printf(".\n");
	}
}

